#include "RigidBody.h"

void Physics::RigidBody::SetMass(const float &mass)
{
    m_Impl->setMass(mass);
};

float Physics::RigidBody::GetMass()
{
    return m_Impl->getMass();
}

bool Physics::RigidBody::IsActive()
{
    return m_Impl->isActive();
}
void Physics::RigidBody::SetIsActive(const bool v)
{
    m_Impl->setIsActive(v);
}

void Physics::RigidBody::SetTransform(const glm::vec3 &pos, const glm::quat &rot)
{
    rp3d::Transform transform;
    transform.setPosition(RP_VEC3(pos));
    transform.setOrientation(RP_QUAT(rot));

    m_Impl->setTransform(transform);
}

void Physics::RigidBody::GetTransform(glm::vec3 &pos, glm::quat &rot)
{
    rp3d::Transform transform = m_Impl->getTransform();
    pos = GLM_VEC3(transform.getPosition());
    rot = GLM_QUAT(transform.getOrientation());
}

Physics::Collider Physics::RigidBody::AddBoxCollider(const glm::vec3 &halfExtends, const glm::vec3 &pos, const glm::quat &rot)
{

    rp3d::Transform transform;
    transform.setPosition(RP_VEC3(pos));
    transform.setOrientation(RP_QUAT(rot));
    auto collider = m_Impl->addCollider(Common::pyCommon.createBoxShape(RP_VEC3(halfExtends)), transform);
    return Collider(collider);
}

Physics::Collider Physics::RigidBody::AddSphereCollider(const float &radias, const glm::vec3 &pos, const glm::quat &rot)
{
    rp3d::Transform transform;
    transform.setPosition(RP_VEC3(pos));
    transform.setOrientation(RP_QUAT(rot));
    auto collider = m_Impl->addCollider(Common::pyCommon.createSphereShape(radias), transform);
    return Collider(collider);
}

Physics::Collider Physics::RigidBody::AddCapsuleCollider(const float &radius, const float &height, const glm::vec3 &pos, const glm::quat &rot)
{
    rp3d::Transform transform;
    transform.setPosition(RP_VEC3(pos));
    transform.setOrientation(RP_QUAT(rot));

    auto collider = m_Impl->addCollider(Common::pyCommon.createCapsuleShape(radius, height), transform);
    return Collider(collider);
}

Physics::Collider Physics::RigidBody::AddHeightFieldCollider(int nbGridColumns, int nbGridRows, float minHeight,
                                                             float maxHeight, const void *heightFieldData, Physics::RigidBody::HeightDataType dataType, int upAxis, float integerHeightScale, const glm::vec3 &scaling, const glm::vec3 &pos, const glm::quat &rot)
{
    rp3d::Transform transform;
    transform.setPosition(RP_VEC3(pos));
    transform.setOrientation(RP_QUAT(rot));

    auto shape = Common::pyCommon.createHeightFieldShape(
        nbGridColumns, nbGridRows, minHeight,
        maxHeight, heightFieldData, (rp3d::HeightFieldShape::HeightDataType)dataType,
        upAxis, integerHeightScale, RP_VEC3(scaling));

    auto collider = m_Impl->addCollider(shape, transform);
    return Collider(collider);
}

Physics::Collider Physics::RigidBody::AddConvexMeshCollider(glm::vec3 *vertices, unsigned int numVertices, int *indices, unsigned int numIndices, unsigned int numFaces, unsigned int vertexStrideOfFace,
                                                            const glm::vec3 &meshScaling,
                                                            const glm::vec3 &pos, const glm::quat &rot)
{
    rp3d::Transform transform;
    transform.setPosition(RP_VEC3(pos));
    transform.setOrientation(RP_QUAT(rot));

    using namespace rp3d;

    PolygonVertexArray ::PolygonFace *polygonFaces = new PolygonVertexArray ::PolygonFace[numFaces];
    PolygonVertexArray ::PolygonFace *face = polygonFaces;
    for (int f = 0; f < numFaces; f++)
    {
        face->indexBase = f * vertexStrideOfFace;
        face->nbVertices = vertexStrideOfFace;
        face++;
    }
    // Create the polygon vertex array
    PolygonVertexArray *polygonVertexArray = new PolygonVertexArray(numVertices, &vertices[0].x, sizeof(glm::vec3),
                                                                    indices, sizeof(int), numFaces, polygonFaces,
                                                                    PolygonVertexArray ::VertexDataType ::VERTEX_FLOAT_TYPE,
                                                                    PolygonVertexArray ::IndexDataType ::INDEX_INTEGER_TYPE);

    auto polyhedronMesh = Common::pyCommon.createPolyhedronMesh(polygonVertexArray);
    auto shape = Common::pyCommon.createConvexMeshShape(polyhedronMesh, RP_VEC3(meshScaling));

    auto collider = this->m_Impl->addCollider(shape, transform);

    delete[] polygonFaces;
    delete polygonVertexArray;

    return Collider(collider);
}

Physics::Collider Physics::RigidBody::AddConcaveMeshColllider(glm::vec3 *vertices, unsigned int numVertices, int *indices, unsigned int numIndices, const glm::vec3 &meshScaling, const glm::vec3 &pos, const glm::quat &rot)
{

    rp3d::Transform transform;
    transform.setPosition(RP_VEC3(pos));
    transform.setOrientation(RP_QUAT(rot));

    using namespace rp3d;

    TriangleVertexArray *triangleArray =
        new TriangleVertexArray(numVertices, &vertices[0].x,
                                sizeof(glm::vec3), numIndices / 3,
                                indices, 3 * sizeof(int),
                                TriangleVertexArray ::VertexDataType ::VERTEX_FLOAT_TYPE,
                                TriangleVertexArray ::IndexDataType ::INDEX_INTEGER_TYPE);

    TriangleMesh *triangleMesh = Common::pyCommon.createTriangleMesh();
    triangleMesh->addSubpart(triangleArray);
    auto shape = Common::pyCommon.createConcaveMeshShape(triangleMesh, RP_VEC3(meshScaling));

    delete triangleArray;
    
    auto collider = this->m_Impl->addCollider(shape, transform);
}


void Physics::RigidBody::SetBodyType(const Type &type)
{
    m_Impl->setType((rp3d::BodyType)type);
};

Physics::RigidBody::Type Physics::RigidBody::GetBodyType()
{
    return (Type)m_Impl->getType();
}

glm::vec3 Physics::RigidBody::GetLinearVelocity() const
{
    return GLM_VEC3(this->m_Impl->getLinearVelocity());
}

void Physics::RigidBody::SetLinearVelocity(const glm::vec3 &linearVelocity)
{
    this->m_Impl->setLinearVelocity(RP_VEC3(linearVelocity));
}

glm::vec3 Physics::RigidBody::GetAngularVelocity() const
{
    return GLM_VEC3(this->m_Impl->getAngularVelocity());
}

void Physics::RigidBody::SetAngularVelocity(const glm::vec3 &angularVelocity)
{
    this->m_Impl->setAngularVelocity(RP_VEC3(angularVelocity));
}

const glm::vec3 &Physics::RigidBody::GetLocalInertiaTensor() const
{
    auto res = this->GetLocalCenterOfMass();
    return GLM_VEC3(res);
}

void Physics::RigidBody::SetLocalInertiaTensor(const glm::vec3 &inertiaTensorLocal)
{
    this->m_Impl->setLocalInertiaTensor(RP_VEC3(inertiaTensorLocal));
}

const glm::vec3 &Physics::RigidBody::GetLocalCenterOfMass() const
{
    return GLM_VEC3(m_Impl->getLocalCenterOfMass());
}

void Physics::RigidBody::SetLocalCenterOfMass(const glm::vec3 &centerOfMass)
{
    m_Impl->setLocalCenterOfMass(RP_VEC3(centerOfMass));
}

bool Physics::RigidBody::IsGravityEnabled() const
{
    return m_Impl->isGravityEnabled();
}

void Physics::RigidBody::EnableGravity(bool isEnabled)
{
    m_Impl->enableGravity(isEnabled);
}

bool Physics::RigidBody::IsSleeping() const
{
    return m_Impl->isSleeping();
}

void Physics::RigidBody::SetIsSleeping(bool isSleeping)
{
    m_Impl->setIsSleeping(isSleeping);
}

float Physics::RigidBody::GetLinearDamping() const
{
    return m_Impl->getLinearDamping();
}

void Physics::RigidBody::SetLinearDamping(float linearDamping)
{
    m_Impl->setLinearDamping(linearDamping);
}

float Physics::RigidBody::GetAngularDamping() const
{
    return m_Impl->getAngularDamping();
}

void Physics::RigidBody::SetAngularDamping(float angularDamping)
{
    m_Impl->setAngularDamping(angularDamping);
}

const glm::vec3 &Physics::RigidBody::GetLinearLockAxisFactor() const
{
    return GLM_VEC3(m_Impl->getLinearLockAxisFactor());
}

void Physics::RigidBody::SetLinearLockAxisFactor(const glm::vec3 &linearLockAxisFactor) const
{
    m_Impl->setLinearLockAxisFactor(RP_VEC3(linearLockAxisFactor));
}

const glm::vec3 &Physics::RigidBody::GetAngularLockAxisFactor() const
{
    return GLM_VEC3(m_Impl->getAngularLockAxisFactor());
}

void Physics::RigidBody::SetAngularLockAxisFactor(const glm::vec3 &angularLockAxisFactor) const
{
    m_Impl->setAngularLockAxisFactor(RP_VEC3(angularLockAxisFactor));
}

Physics::RigidBody::RigidBody(rp3d::RigidBody *impl)
    : m_Impl(impl)
{
}
